/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2024 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdio.h>   // for printf, NULL
#include <stdlib.h>  // for free, malloc, calloc

#include "ConvexQP.h"               // for ConvexQP
#include "ConvexQP_Solvers.h"       // for convexQP_ProjectedGradient, conve...
#include "ConvexQP_computeError.h"  // for convexQP_compute_error_reduced
#include "ConvexQP_cst.h"           // for SICONOS_CONVEXQP_PGOC_RHO, SICONO...
#include "NumericsFwd.h"            // for SolverOptions, ConvexQP, Numerics...
#include "NumericsMatrix.h"         // for NM_gemv
#include "SiconosBlas.h"            // for cblas_dcopy, cblas_daxpy, cblas_ddot
#include "SolverOptions.h"          // for SolverOptions, solver_options_nul...
#include "numerics_verbose.h"       // for numerics_error, verbose, numerics...
#include "siconos_debug.h"          // for DEBUG_PRINTF

//#define VERBOSE_DEBUG
const char* const SICONOS_CONVEXQP_PG_STR = "CONVEXQP PG";

void convexQP_ProjectedGradient(ConvexQP* problem, double* z, double* w, int* info,
                                SolverOptions* options) {
  /* int and double parameters */
  int* iparam = options->iparam;
  double* dparam = options->dparam;

  double* q = problem->q;
  NumericsMatrix* M = problem->M;
  NumericsMatrix* A = problem->A;

  if (!A) {
    numerics_warning("ConvexQP_ProjectedGradient",
                     "This solver does not support a specific matrix A different from the "
                     "identity and b=0");
  }

  /* Dimension of the problem */
  int n = problem->size;

  if (!problem->istheNormConvexQPset) {
    problem->normConvexQP = cblas_dnrm2(n, problem->q, 1);
    DEBUG_PRINTF("problem->norm ConvexQP= %12.8e\n", problem->normConvexQP);
    problem->istheNormConvexQPset = 1;
  }
  double norm_q = problem->normConvexQP;
  DEBUG_PRINTF("norm_q = %12.8e\n", norm_q);
  /* Maximum number of iterations */
  int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
  /* Tolerance */
  double tolerance = dparam[SICONOS_DPARAM_TOL];

  /*****  Projected Gradient iterations *****/
  int iter = 0;      /* Current iteration number */
  double error = 1.; /* Current error */
  int hasNotConverged = 1;

  double rho = 0.0;
  double rhomin = 0.0;
  int isVariable = 0;

  if (dparam[SICONOS_CONVEXQP_PGOC_RHO] > 0.0) {
    rho = dparam[SICONOS_CONVEXQP_PGOC_RHO];
  } else {
    /* Variable step in fixed*/
    isVariable = 1;
    if (verbose > 0) printf("Variable step (line search) in Projected Gradient iterations\n");
    rho = -dparam[SICONOS_CONVEXQP_PGOC_RHO];
    rhomin = dparam[SICONOS_CONVEXQP_PGOC_RHOMIN];
  }

  if (rho == 0.0)
    numerics_error("ConvexQP_ProjectedGradient",
                   "dparam[SICONOS_CONVEXQP_PGOC_RHO] must be nonzero");

  double rho_k = rho;
  double* z_tmp = (double*)malloc(n * sizeof(double));
  double* z_k;
  double* direction;
  double* w_k;
  if (isVariable) {
    z_k = (double*)malloc(n * sizeof(double));
    direction = (double*)malloc(n * sizeof(double));
    w_k = (double*)malloc(n * sizeof(double));
  }
  double alpha = 1.0;
  double beta = 1.0;

  if (!isVariable) {
    double minusrho = -1.0 * rho;

    while ((iter < itermax) && (hasNotConverged > 0)) {
      ++iter;

      /* q --> w */
      cblas_dcopy(n, q, 1, w, 1);

      /* M z + q --> w */
      NM_gemv(alpha, M, z, beta, w);

      /* projection for each contact of z - rho * w  */
      cblas_daxpy(n, minusrho, w, 1, z, 1);

      cblas_dcopy(n, z, 1, z_tmp, 1);
      problem->ProjectionOnC(problem, z_tmp, z);

      /* **** Criterium convergence **** */
      // Warning : options->dWork required in the function below !
      convexQP_compute_error_reduced(problem, z, w, tolerance, options, norm_q, &error);

      if (verbose > 0)
        printf(
            "--------------- ConvexQP - Projected Gradient (PG) - Iteration %i rho = %14.7e "
            "\tError = %14.7e\n",
            iter, rho, error);

      if (error < tolerance) hasNotConverged = 0;
      *info = hasNotConverged;
    }
  } else {
    double tau = dparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_TAU];
    if ((tau <= 0.0) || (tau >= 1.0))
      numerics_error("fc3d_ProjectedGradientOnCylinder",
                     "dparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_TAU] must in (0,1)");

    double mu = dparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_MU];
    if ((mu <= 0.0) || (mu >= 1.0))
      numerics_error("fc3d_ProjectedGradientOnCylinder",
                     "dparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_MU] must in (0,1)");

    int ls_iter_max = iparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_MAX_ITER];

    double theta = 0.0;
    double theta_k = 0.0;
    double correction = 0.0;
    double criterion = 1.0;

    int ls_iter = 0;
    // verbose=2;

    cblas_dcopy(n, z, 1, z_tmp, 1);
    problem->ProjectionOnC(problem, z_tmp, z);

    /* store the old z */
    cblas_dcopy(n, z, 1, z_k, 1);

    /* q --> velocity_k */
    cblas_dcopy(n, q, 1, w_k, 1);
    /* M r + q --> velocity_k */
    NM_gemv(1.0, M, z, 1.0, w_k);

    /* Compute the value fo the cost function (we use w as tmp) */
    /* w_k --> w */
    cblas_dcopy(n, w_k, 1, w, 1);
    /* M z + 2* q --> w */
    cblas_daxpy(n, 1.0, q, 1, w, 1);
    theta_k = 0.5 * cblas_ddot(n, z, 1, w, 1);

    while ((iter < itermax) && (hasNotConverged > 0)) {
      rho = rho_k;
      ++iter;

      /* Armijo line-search */
      criterion = 1.0;
      ls_iter = 0;
      while ((criterion > 0.0) && (ls_iter < ls_iter_max)) {
        if (verbose > 1) printf("ls_iter = %i\t rho = %e\t", ls_iter, rho);

        // Compute the new trial reaction
        // r = P_D(r_k - rho v_k)
        cblas_dcopy(n, z_k, 1, z, 1);
        cblas_daxpy(n, -1.0 * rho, w_k, 1, z, 1);

        cblas_dcopy(n, z, 1, z_tmp, 1);
        problem->ProjectionOnC(problem, z_tmp, z);

        //  Compute the new value of the cost function (we use velocity as tmp)
        /* q --> w */
        cblas_dcopy(n, q, 1, w, 1);

        cblas_daxpy(n, 1.0, q, 1, w, 1);
        NM_gemv(1.0, M, z, 1.0, w);

        /* M r + 2*q --> w */
        // NM_gemv(1.0, M, z, 2.0, w);

        /* theta = 0.5 R^T M r + r^T q */
        theta = 0.5 * cblas_ddot(n, w, 1, z, 1);

        // Compute direction d_k = z - z_k
        cblas_dcopy(n, z, 1, direction, 1);
        cblas_daxpy(n, -1, z_k, 1, direction, 1);

        correction = mu * cblas_ddot(n, w_k, 1, direction, 1);
        criterion = theta - theta_k - correction;
        if (verbose > 1)
          printf("theta = %e\t theta_k = %e\t  correction = %e\t criterion = %e\n", theta,
                 theta_k, correction, criterion);
        rho = rho * tau;
        ls_iter++;
        if (rho < rhomin) break;
      }
      /* store the old z */
      cblas_dcopy(n, z, 1, z_k, 1);
      /* Compute the value fo the cost function (we use w as tmp) */
      /* q --> velocity_k */
      cblas_dcopy(n, q, 1, w_k, 1);
      /* M r + q --> velocity_k */
      NM_gemv(1.0, M, z, 1.0, w_k);
      /* w_k --> w */
      cblas_dcopy(n, w_k, 1, w, 1);
      /* M z + 2* q --> w */
      cblas_daxpy(n, 1.0, q, 1, w, 1);
      theta_k = 0.5 * cblas_ddot(n, z, 1, w, 1);
      rho = rho / tau;
      /* **** Criterium convergence **** */
      convexQP_compute_error_reduced(problem, z, w, tolerance, options, norm_q, &error);

      if (verbose > 0)
        printf(
            "--------------- ConvexQP - Projected Gradient (PG) - Iteration %i rho_k = %10.5e "
            " rho =%10.5e error = %10.5e < %10.5e\n",
            iter, rho_k, rho, error, tolerance);

      /* Try to increase rho is the ls_iter succeeds in one iteration */
      if (ls_iter == 1) {
        if (verbose > 1)
          printf(
              "--------------- ConvexQP - Projected Gradient (PG) -  rho_k is increased from "
              "%10.5e to %10.5e \n",
              rho_k, rho_k / tau);
        rho_k = rho_k / tau;
      }
      if (error < tolerance) hasNotConverged = 0;
      *info = hasNotConverged;
    }
  }

  // verbose=1;

  if (verbose > 0)
    printf(
        "---------------  ConvexQP - Projected Gradient (PG) - #Iteration %i Final Residual = "
        "%14.7e\n",
        iter, error);
  dparam[SICONOS_DPARAM_RESIDU] = error;
  iparam[SICONOS_IPARAM_ITER_DONE] = iter;

  if (z_tmp) free(z_tmp);
  z_tmp = NULL;
  if (isVariable) {
    free(z_k);
    free(direction);
    free(w_k);
  }
}

void convexQP_ProjectedGradient_set_default(SolverOptions* options) {
  options->iparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_MAX_ITER] = 20;
  options->dparam[SICONOS_CONVEXQP_PGOC_RHO] = -1.e-3; /* rho is variable by default */
  options->dparam[SICONOS_CONVEXQP_PGOC_RHOMIN] = 1e-9;
  options->dparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_MU] = 0.9;
  options->dparam[SICONOS_CONVEXQP_PGOC_LINESEARCH_TAU] = 2.0 / 3.0;
}
